
/**
 * @file LESO.hpp
 */

#pragma once

#include <matrix/matrix/math.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/leso_value.h>

class LESO
{
public:
	LESO();
	~LESO() = default;
	void set_wo(const float & _wo);
	matrix::Vector3f  get_e_estimate();
	matrix::Vector3f  get_rate_estimate();
	matrix::Vector3f  get_disturbance_estimate();
	matrix::Vector3f  calculate_e(const matrix::Vector3f &rate_sp);
	void run(const matrix::Vector3f &rate_sp, const matrix::Vector3f &rate, const float dt);
	void set_aux_para(const matrix::Vector3f &proportional_gain, const float yaw_weight);
	void reset();
	void set_last_U(const matrix::Vector3f &u);
private:

	void update_phi_and_tau(const float dt);
	float wo{10.0};
	matrix::Vector3f  Z[3];  //LESO状态


	matrix::Matrix3f  A;   //连续状态方程中的A
	matrix::Matrix3f  B;  //连续状态方程中的B
	matrix::Matrix3f  PHI;
	matrix::Matrix3f  TAU;
	matrix::Vector3f J{0.029f,0.011f,0.042f};  //转动惯量矩阵
	// float Jxx = 0.029f;
	// float Jyy = 0.011f;
	// float Jzz = 0.042f;
	matrix::Vector3f coef{154.0f, 273.5f, 12.85f};
	matrix::Vector3f  last_U;
	matrix::Vector3f  att_p;

	uORB::Publication<leso_value_s>   _leso_value{ORB_ID(leso_value)};
};
